#include <Arduino.h>
#include <MPU6050_tockn.h>
#include <Wire.h>

#include <SimpleFOC.h>
#include <SCServo.h> //舵机库

float pitch;
float Mechanical_balance = 3.0;//实际测得的机械中值 
float target,kp1;
int M1Dir,M2Dir;

MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
MagneticSensorI2C sensor2 = MagneticSensorI2C(AS5600_I2C);

// Motor instance
BLDCMotor motor1 = BLDCMotor(7);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(32,33,25,22);

BLDCMotor motor2 = BLDCMotor(7);
BLDCDriver3PWM driver2  = BLDCDriver3PWM(26,27,14,12);

// velocity set point variable
float target_velocity1 = 0;
float target_velocity2 = 0;

float target_voltage1 = 0;
float target_voltage2 = 0;
// instantiate the commander
Commander command = Commander(Serial);
void doTarget1(char* cmd) { command.scalar(&target_voltage1, cmd); }
void doTarget2(char* cmd) { command.scalar(&target_voltage2, cmd); }


//MPU6050实例
MPU6050 mpu6050(Wire1);   //Wire1对应世博的I2Ctwo

// 舵机实例
SMS_STS st;

#define S_RXD 16
#define S_TXD 17   //这两引脚实际即为串口2的默认引脚  



void setup() {
  Serial.begin(115200);
  Wire1.begin(23,5,400000UL);  //Wire1对应世博的I2Ctwo
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);

   // initialise magnetic sensor hardware
  Wire.begin(19,18, 400000UL); //Wire对应世博的I2Cone
  sensor1.init(&Wire);
  sensor2.init(&Wire1);
  // link the motor to the sensor
  motor1.linkSensor(&sensor1);
  motor2.linkSensor(&sensor2);

  // driver config
  // power supply voltage [V]
  driver1.voltage_power_supply = 8;
  driver1.init();

  driver2.voltage_power_supply = 8;
  driver2.init();
  // link the motor and the driver
  motor1.linkDriver(&driver1);
  motor2.linkDriver(&driver2);
  // choose FOC modulation (optional)
  motor1.foc_modulation = FOCModulationType::SpaceVectorPWM;
  motor2.foc_modulation = FOCModulationType::SpaceVectorPWM;
  // set motion control loop to be used
  motor1.controller = MotionControlType::torque;
  motor2.controller = MotionControlType::torque;
  // contoller configuration
  // default parameters in defaults.h

  // velocity PI controller parameters
  motor1.PID_velocity.P = 0.2;
  motor1.PID_velocity.I = 2;

  motor2.PID_velocity.P = 0.2;
  motor2.PID_velocity.I = 2;
  // maximal voltage to be set to the motor
  motor1.voltage_limit = 4;
  motor1.voltage_sensor_align = 2;//零点校准对齐电压（力矩）

  motor2.voltage_limit = 4;
  motor2.voltage_sensor_align = 2;//零点校准对齐电压（力矩）
  // velocity low pass filtering time constant
  // the lower the less filtered
  motor1.LPF_velocity.Tf = 0.05f;
  motor2.LPF_velocity.Tf = 0.05f;
  // angle P controller
  motor1.P_angle.P = 20;
  motor2.P_angle.P = 20; 
  // maximal velocity of the position control
  motor1.velocity_limit = 40;
  motor2.velocity_limit = 40;
  // use monitoring with serial
  //Serial.begin(115200);
  // comment out if not needed
  motor1.useMonitoring(Serial);
  motor2.useMonitoring(Serial);

  // initialize motor
  motor1.init();
  motor2.init();
  // align sensor and start FOC
  motor1.initFOC();
  motor2.initFOC();

  // add target command T
 command.add('V', doTarget1, "target voltage1");
 command.add('T', doTarget2, "target voltage2");

  Serial.println(F("Motor ready."));
  Serial.println(F("Set the target angle using serial terminal:"));
  _delay(1000);


  Serial2.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
  st.pSerial = &Serial2;
  delay(1000);


  st.RegWritePosEx(1, 2558, 3400, 50); // servo(ID1) speed=3400，acc=50，move to position=2558.
  st.RegWritePosEx(2, 1527, 3400, 50); // servo(ID2) speed=3400，acc=50，move to position=1527.
  st.RegWriteAction();
  delay(3000); //最高

  st.RegWritePosEx(1, 2056, 3400, 50); // servo(ID1) speed=3400，acc=50，move to position=2056.
  st.RegWritePosEx(2, 2044, 3400, 50); // servo(ID2) speed=3400，acc=50，move to position=2044.
  st.RegWriteAction();
  delay(3000); //最低

  st.RegWritePosEx(1, 2558-300, 3400, 50); // servo(ID1) speed=3400，acc=50，move to position=2056.
  st.RegWritePosEx(2, 1527+300, 3400, 50); // servo(ID2) speed=3400，acc=50，move to position=2044.
  st.RegWriteAction();


  M1Dir = 1;
  M2Dir = 1;
  kp1 = 0.18;//该P值需要精调节

  delay(3000); //固定一个中间位置
}

void loop()
 {
  mpu6050.update();
  // Serial.print("angleX : ");
  // Serial.print(mpu6050.getAngleX());
  //Serial.print("\tangleY : ");
  //Serial.print(mpu6050.getAngleY());
  // Serial.print("\tangleZ : ");
  // Serial.println(mpu6050.getAngleZ());
  pitch = mpu6050.getAngleY();
  target = kp1*(Mechanical_balance-pitch);



  motor1.loopFOC();
  motor1.move(M1Dir*target);

  motor2.loopFOC();
  motor2.move(M2Dir*target);

  //command.run();

}